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Abstract 

Collision-free optimal motion and trajectory planning for robotic 
manipulator! are solved by a method of icquential gradient restoration 
algorithm. Numerical examples of a two degree-of-freedom (DOF) robotic 
manipulator are demonstrated to show the excellency of the optimisation 
technique and obstacle avoidance scheme. The obstacle is put on the 
midway, or even further inward on purpose, of the previous no*obstacle 
optimal trajectory, For the minimum-time purpose, the trajectory grazes 
by the obstacle and the minimum-time motion successfully avoids the 
obstacle. The minimum-time is longer for the obstacle avoidance cases 
than the one without obstacle. The obstacles avoidance scheme can deal 
with multiple obstacles in any ellipsoid forms by using artificial potential 
fields as penalty functions via distance functions. The method it promising 
in solving collision-free optimal control problems for robotics and can be 
applied to any DOF robotic manipulators with any performance indices and 
mobile robots as well. Since this method generates optimum solution based 
on Pontryagin Extremum Principle, rather than based on assumptions, the 
results provide a benchmark against which any optimization technique* 
can be measured. 

Key words Bang-bang control, optimal control, Cartesian space, joint 
space, robotic manipulators, degree-of-freedom. 

1. Introduction 

The problem of increasing productivity, automated manufacturing, 
and performing complex tasks in hazardous or remote environments can 
be solved by robotic systems. Such systems have been applied to a wide 
variety of industries which includes spray painting, welding, assembling, 
material handling, highly risky work and remote control jobs. As pointed 
out by Holcomb and Montemerlo [1] and Lemer [2], remote control robotic 
systems will be developed in the future space stations. Also si well- 
known, with the demand of increasing productivity and industrial 
automation, the problem of controlling the robotic manipulators has 
received a great deal of interest in the field of automated manufacturing. 

1.1 Research Objectives 

One of the focal points in robot design lies in the computation and 
control of the motion of the manipulator. In order to make sure that the 
manipulator is able to execute a special task most efficiently for human 
beings, it is important that the manipulator performi from initial state* to 
designated final states in an optimal way under collision avoidance 
concern. Control on robotics can be separated into two major categories: 1) 
trajectory planning, 2) trajectory tracking. Various optimal controllers 
need to be devised in the trajectory tracking problems which are not the 
subjects in this article. Trajectory planning is not only the determination of 
the path of the end effector. Trajectory planning generates a specified 
motion of time history from initial states to final siaiei. Motion planning 
does not necessarily require optimization techniques but extra excursion of 
the robot Is just not cost-efficient and can cause more potential collision 
problems. Obviously, the minimum-time trajectory Is of particular interest 
since the productivity in automation is maximized. Various performance 
goals, for example: distance, energy or time-energy combination, ire alio 
applicable. Various concepts for the study of optimal control of robotic 
manipulators have been studied for this purpose. 

1.2 Prevloui Work 

One of the pioneered work is done by Kahn and Roth [3]. The highly 
nonlinear manipulator dynamical equation* of motion are linearized, an 
approximate bang-bang solution has been developed to the suboptimal 
feedback control problem. 

Gilbert and Johnson [4] have developed a path planning scheme in 
which the obstacles are avoided via an infinite penalty function generated 
from distance function. In their study, the nonlinear dynamic equations 
are approximated by linear subipace functions which are chosen as 
piecewise polynomial splines. In their examples, distance constraints are 


violated when spline knot interval sections equal to one; the payload object 
Is made strictly convex by approximating its boundary by arcs of certain 
curvature; obstacles are assumed to be convex sets; the complex distance 
finding minimization problem within the optimal control problem is not 
fully described. In the optimization technique, more than one optima can 
be drawn at the same case. 

Based on Pontryagin extremum principle, the time-optimal motions 
of various types of robotic manipulators have been investigated by 
Geering, Guzzella, Hepner and Onder [5] as classified by cylindrical, 
spherical robots, and a robot with horizontal articulated arm with two 
links. In the analysis of the time-optimal control problem, the bang-bang 
control solution satisfies the Pontryagin extremum principle and the study 
has been made for uncon suainted trajectories. In their examples, two link* 
intercross each other in the planar two-link manipulator. 

Due to the difficulty of highly nonlinear robot mathematical model, a 
near-optimal control algorithm based on Pontryagin extremum principle 
and Riccati formulation has been presented by Kim, Jamshidi and 
Shahinpoor [€}. The algorithm reduces the original nonlinear equation set 
into a linear one by a parameter sensitivity method and P-D controller is 
used to solve the linearized model. 

Ozaki and Mohri [71 has developed the study of collision-free joint 
trajectories along a given path with some physical constraints such as 
manipulator dynamics, obstacles avoidance, joint velocities and input 
torques by formulating artificial potentials into the planning problem for 
constraints using linear programming algorithm to minimize the error 
between present and desired trajectory, in which, the nonsmooth time 
functions were approximated by cubic spline functions. 

The technique of dynamic programming has also been a popular 
solution method to many investigators in the field of robotic manipulators 
research. Based on dynamic programming, Vukobratovic and Kircanski [81 
have determined the energy-optimal velocity distribution of the 
manipulator end-effector for a prescribed path in the workspace subject to 
the forces/torques constraints. The given traveling time needs to be 
discretized in their study. 

Singh and Leu [9] have formulated and solved the optimal trajectory 
planning as an optimal control problem by a path parameterized method of 
dynamic programming under the constraints of the joint forces/torques 
and velocities. Bang bang control has been generated for minimum time 
problems without obstacles avoidance concent. 

In order to implement dynamic programming approach, Shin and 
McKay [10] have studied trajectory planning of robotic manipulator* using 
parametric function and its derivative to reduce dimensions in state space 
which find* the positions, velocities, accelerations, and torques of the 
problem by minimizing the cost of the parameter of moving a robotic 
manipulator along a specified geometric path subject to input torque/force 
constraints without obstacle avoidance concern. Along a pre- selected 
geometric path, for quadratic velocity bounds, and piecewise analytic 
geometric path constraints, the minimum-time control problem has been 
studied by Shin and McKay [111 with the phase-plane techniques In 
Cartesian space which has to be converted Into joint space by Interpolation. 
Under the assumption that the path is given as parameterized curve, they 
have also determined a near-minimum time geometric path for the study 
described above which minimizes approximate lower traversal time 

bounds using maximum velocity bounds [1 2). Their techniques are limited 
by parameterization. 

Bobrow, Dubowsky, and Gibson [13] have studied the problem of 
minimum-time trajectories along arbitrary pre-planned spatial paths by a 
special technique in which the actuator torque bounds are assumed to be 
function* of the robot's current position and velocity. This technique 

cannot handle the case when the feasible regions in the phase plane are 

not simply connected. The idea of the time-optimal solution is based on 
choosing the maximum acceleration/deccleralion to make velocity as large 
as possible at every point without violating constraints. The difficulty Is 
finding multiple switching points for time-optimal problems. Dubowsky, 
Norris and Shiller [14] have devised a lime optimal trajectory planning 
scheme with obstacle avoidance consideration via a CAD approach in which 
the minimum distance to obstacles is found from software OPTARM II by a 
table of various geometric shape. The penalty function for obstacle 

avoidance needs to have a characteristic of more effective weighting and 
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dramatic steepness. The technique cannot be easily extended to solving 
optimal trajectory planning for other performance indices and constraint*. 

Based on the same assumption, Rajan [15] has devised a 
parameterized path method for the minimum-time problem in which the 
cubic spline paths arc parameterized and optimized locally by an iterative 
scheme. The optimization procedure stops until the minimum-time path 
comes close enough to the previous path while using Bobrow's algorithm 
for inner minimization and varying the path for outer minimization The 
algorithm cannot be effectively applied to the planar movements of a 
manipulator with obstacles in the workspace. The weak points of the 
algorithm arc on the premises that the minimum time path is smooth and 
a smooth curve is well approximated by splines. 

Sahar and Hollerbach [16] have devised a method based on slate - 
space search tree representing all possible solutions, and searching for the 
best one by using a Symbolics Lisp Machine for time-minimum criterion. 
The algorithm is I logical approach but not a mathematical approach which 
is not suitable for routine off-line trajectory planning due to the 
complexity of computation, - 

Luh and Lin [17] have devised a kinematical approach which 
assumes the path consists of a sequence of Cartesian straight line segments 
and constant limits on Cartesian velocity and acceleration are known a 
priori without considering the dynamics of the arm. 

Weinreb and Bryson [18] have presented the Adjustable Control- 
Variation Weight (ACW) algorithm for the minimum-time control of a two- 
link robotic arm through choosing controls subject to the actuator 
constraints. In their examples, the two links of the planar manipulator 
intercross each other. Meier and Bryson [19] have developed an algorithm 
for solutions of time-optimal control problem of a two-link planar 
manipulator which contains solutions for two-point boundary value 
problem of consirainted motion between two endpoints. 

Zhang and Wang [20] have investigated a collision-free time-optimal 
control problem of a two-link planar robotic manipulator by applying the 
method of global linearization transformation in joint space configuration. 
As a result, the nonlinear equations of motion arc transformed into an 
equivalent linear model and an approximate explicit expression has been 
obtained for the case of minimum-lime control of a two-link planar robotic 
manipulator with two-dimensional planar geometrical obstacle avoidance. 
In their example, radius of the circle obstacle is not shown. 

Bobrow [21] has continued the study of optimal path planning using 
minimum-time criterion with obstacles avoidance consideration in which 
the actuator torque bounds are assumed to be functions of the robot's 
current position and velocity, where the Cartesian path of the end-effector 
is represented with uniform cubic B spline polynomials. The obstacle 
avoidance is enforced by ensuring the distance between the end-effector 
and the obstacle which was evaluated by stepping small increments of the 
path parameter. 

Wang [22] has devised the numerically approach of using sequential 
gradient restoration algorithm to solve Bolza classical optimal control 
problem on robotics without linearization or parameterization, including 
the analytical time -optimal solutions of a two-link manipulator and/or 
actuator consirainted cases, in which the implementation can be extended 
into obstacle avoidance consideration, 

1,3 Overview 

We can see that numerous attempts have been made to find 
collision-free optimal motion of a robotic manipulator without great 
success. All of the aforementioned investigations arc limited in one way or 
another. 

Collision-free optimal control problems for robotic manipulators are 
difficult due to the two-point boundary-value problem which involves* in 
addition to the optimality conditions, the kinematical and highly nonlinear 
dynamical equations of the systein, Ihe obstacle constraints, the limits 
imposed on controls, and the satisfaction of terminal conditions. Generally 
speaking, analytical solutions for classical optimal control problems with 
equality and/or inequality constraints are not possible. Therefore, 
numerical method is resolved. Numerical methods and computer routines 
are available nowadays ringing from simple integration to TPBVP and 
optimization at a low price [23]. 

To solve consirainted optimal control problems, a restoration phase is 
needed at the end of the gradient phase [24]. Hie collision-free motion 
planning problems of robotics can be formulated as a classical optimal 
control problem and solved by sequential gradient restoration algorithm 
[25]. Collision can be avoided by continuously controlling the closest point 
on the arm to the obstacles using virtual potential fields as penalty 
functions via distance functions [26]. 


1.4 Present Modeling 

As pointed out in recent research, owing to the difficulty of solving 
TPBVP and highly nonlinear dynamic equations, the classical optimal 
control problem is mostly approached by approximation (linearization, 
parameterization, modification) which more or less replaces the original 
optimal control problem into the assumed one. As in those study where the 
nonlinear dynamic equations or the two-point boundary value problems 
are linearized or parameterized, the solutions generated based on those 
assumptions are not necessarily good approximations to the original ones. 
The intention of this research is to present a numerical approach for 
determining the collision -free optimum motion of robotic manipulators, a 
method to solve classical optima! control problem without any 
modification, linearization or simplification. Solutions including robot 
positions, velocities, accelerations and force/torquc in both Cartesian space 
and joint space which satisfies the Pontryagin extremum principle are 
obtained by solving the manipulator kinematical and dynamical equations 
with optimality conditions. For given inirial and final conditions, under the 
physical conditions imposed on control in join! space and obstacles 
constraints, the continuous time-history of the positions, velocities, 
accelerations, torques/forces and the optima! collision-free motion of a 
robotic manipulator in minimum time are determined. 

Applications of sequential gradient restoration algorithm occur in 
various branches of science and engineering. With particular regards to 
aerospace engineering, various problems of coplanar and noncoplanar, 
orbital and suborbital space flight [27, 28, 29} and atmospheric flight in a 
windshear [30, 31, 32] have been solved by the sequential gradient 
restoration algorithm. Also, the same technique has been successfully 
employed in the ihermofluid science [33, 34]. In general, sequential 
gradient restoration algorithm has proven to be a very promising 
algorithm in solving engineering optimal control problems [35, 36J. 

1.5 Advantages over Existing Techniques 

To solve collision-free optimal control problems on robotics with 
constraints, we need a numerical method which has the following 
advantages: 

1) able to solve TPBVP which is essentially the core of the problem [6, 8, 
10, II, 12, 13, 14, 15, 21]: In fact, TPBVP can be solved by shooting 
method and relaxation method or method of particular solutions, TPBVP is 
involved in the first-order exact optimality conditions derived from 
calculus of variation. 

2) able to solve highly nonlinear dynamic equations without linearization, 
parameterization or simplification [3, 4, 6, 7, 10, 11, 12, 17, 20, 21): Any 
modification by linearization or simplification directly or indirectly 
replaces the original problem. The drastic approximation leads to 
significant error and unsatisfactory, unknown effect to the optima and the 
obstacle avoidance. For example, a coll is ion -free optimal solution can be 
declared only when there is not another more optimal solution. 

3) able to solve any robotics formulation regardless number of joints or 
DOF [15, 17, 18, 19, 20]: A technical approach should not be limited by the 
number of joints or DOF of robotics. Any dynamic systems can be 
formulated from slate functions point of view and solved as control 
systems regardless number of dimensions. 

4) able to avoid the. obstacles toward optimization direction without any 
unnecessary excursion [5, 7, 8, 9, 14, 15, 20, 21 J: Collision avoidance should 
be achieved in a most efficient way, in terms, an optimal way, without 
requiring extra journey of the robot arm. The weighting effect and 
clearence between trajectory and obstacle should be specified by only one 
parameter. 

5) able to solve general constraints of robot motion planning: On trajectory 
planning, we have state constraints, control constraints, or a combination of 
the above. Obstacle inequality constraints, control inequality constraints 
can be transformed into equality constraints. 

6) able to solve any terminal conditions, any performance indices [3, 13, 
14, 15, 18, 21]: In various applications, various performance indices need 
to be implemented. For example, time, distance, energy or a combination of 
the above. Point to point task has different initial and final states in 
applications. 

7) has the potential to fully utilize computer power as the computer 
industries grow in the near future: Several years from now, computers can 
be many Limes faster in CPU, We don’t reject any ideas which consume 
more CPU time than we can afford today. On the contrary, we encourage 
numerical method that fully utilizes the modern scientific computing 
concepts. Provided we have infinitesimal small stepsize and infinite digits, 
and we have sufficient CPU time on computers, this calculus of variation 
approach generates solutions which satisfy exact necessary conditions. 
Sufficient conditions can also be checked. 
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1-7 A^ief'daKription of the dynamic .y.lem. and constraint, are given 
In section 2. Section 3 contain, the ob.lacle. avoidance .cheme., Sectton 4 
l”m”n. the optimal control theory. Section 5 contain, the .equentt.l 
gradient restoration algorithm. In Section 6, numerical examples of a two 
degree-of-freedom robotic manipulator are demonstrated. The insight of 
collision-free minimum-time motion are shown in captions and tables. 
Finally, discussion Is in section 7, conclusion and prospective research are 
presented in section 8. Appendix A illustrates* the kinematics of a two- 
link manipulator example. 


2. Dynamic Systems and Constraints 

The highly nonlinear dynamic equations and inequality control 
constraints and/or inequality state constraints are alio the main 
difficulties of optimal control on robotics. 


2.1 Dynamics 

Under the assumption that the links are uniform rods of mass mi at 
the mass center, of moment of inertia Ij, of length lj, respectively, i is the 
number of the link. The gravity g is acting parallel to the negative y-axis 
direction. The dynamical equations can be derived by means of Newton- 
Euler (Lagrange-Euler) equations [37, 38. 391 or symbolic method [40] and 
expressed in general as: 


2.2 Control Systems and Inequality Control Constraints 
Robotics dynamic system can be formulated in two ways: 

2.2.1 Kinematical Formulation 

Kinemalical fonnulation is practical in most cases, specially when the 
model reference dynamic parameters are not known in advance. For 
example, the payload is never known ahead; or for safety reason that the 
inertia force caused by acceleration of the robots shall be limited. In 
kinematical formulation, the control system is as follows: 


a 

6 = 0 

(5) 

& = a 

(6) 


0, (a, o stc vector of state variables. Once the state* In joint ipace of the 
manipulator are known, we can compute the joint torque* which are 
required to balance the reaction forces/momenli acting on the links. The 
physical inequality constraints Imposed on the robot in this study are joint 
acceleration bounds {20J. With these constraint*, we can limit the torque* 
which are related to the joint space configuration. In terms, 

iaiiC, (7) 

Via the following variable transformation, the joint acceleration can be 
limited within the bounds 

a=Csin(u), ( 8 ) 

C is vector of upper bounds of the absolute acceleration in joint space, uis 
vector of the new control variable. 

2.2.2 Dynamical Formulation 

If we know the model reference system in advance, in dynamical 
formulation, the control system is as follows: 


X =M(9)a + C(9,to) + G(0) 


(I) 


(9) 


where T is the vector of applied torqucs/forces, M(0) is the inertial matrix 
terms of the manipulator, <T (0 ,co) is the vector of centrifugal and Coriolis 
terms, G(0) is the vector of gravity terms. For example, a two-link 
manipulator in Fig. I. [16]: 



Fig.l. Two-link robotic manipulator 
Link 1 of mj, lj , lj 
Link 2 of m 2 , 1 1 2 


/Il+l2+(mil| 2 +m2l2 2 )/4 + m2h 2 +m2lll2COS02 l2+(nt2l2 2 )/ 4 + (m2Ul2Cos02)/2 
M(e)= [ l2+(m2l2 2 )/4+(m 2 lll2Cos02)/2 l2+(™2U 2 )/4 l 2 

( 2 ) 


K -m2lil2S>n02(“2) 2 /2-m2lil 2 sin02(wi)(to2) 
m2Ul2sin02(<«>l) 2 /2 


/ , m2l2COs(0i+02)/2+h(mi/2+m2)cos0i 

G(0)-gl m2l2COs(0i + 02)/2 


(3) 

(4) 


One can see that these highly nonlinear terms are functions of the 
joint velocities and angles. 9i,»i,aiare relative angle, angular velocity, and 
angular acceleration of link i respectively. 


2>=M-!(T -C(0,o)-G(0)) (10) 

0,{o, a are state variables. In this formulation, we assume the dynamic 
parameters in matrices M,C,G, are known. The matrix M is always both 
"symmetric and positive definite" [38], therefore always invertible. Th e 
physical inequality constraints imposed on the robot in this formulation 
are joint torque/forcc bounds. With these constraints, we can limit the 
torques in the actuator space configuration. In terms, 

ItUC, (11) 

Via the following variable transformation, the joint torque can be limited 
within the bounds 


Ti=Cisin(ui)t (1^) 

C is vector of upper bounds of the absolute torque in actuator space, uis 
vector of the new control variable, 

2.3 Equality Constraints 

In some cases, the end-effector has to follow a specified path, or the 
orientation of the arm in the motion is specified and fixed, for example, the 
robot arm is holding a flash tight moving along a specified path, then, the 
degree of freedom is reduced by the number of constraints. One or more 
state constraints have tO_.be added in Cartesian space, then converted into 
joint space. The system is solved with replacement of the algebraic 
equation into the state variables according to the constraints. 

3, Obstacles Avoidance Schemes 

By definition, obstacles can be avoidable or unavoidable for a fixed 
configuration. Configuration has to be fixed in one task to avoid excess 
excursion and changing kinematics. For examples, in Fig. 2., the obstacle is 
away from the robot chassis but within the work envelope. That is 
considered as avoidable. In Fig. 3., obstacles are too close to the robot and 
there is no space for feasiblly moving the robot arm through the obstacle 
environment. This is considered as unavoidable. 

For simplicity, each obstacle is put into an ellipsoid. It is a tilde 
wasteful to pul an obstacle which is not necessarily in ellipsoid shape into 
an ellipse. The advantage is the ellpsoid parameters can be changed to 
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ihapen the oral into the figure of the obstacle without wasting too much 
space. Collision avoidance can be achieved by continuously controlling the 
closest point on the arm to the obstacles. 




Fig. 2. Avoidable Obstacle Fig. 3. Unavoidable Obstacles 

3.1 States Inequality Constraints 

Let Qj denotes the i-th obstacle ellipsoid function among m obstacles. 
The obstacle constraint is: 


Ql - a$(*-xo) 2 + bo(x-xo)(y-yo) + co(y-yo) 2 + fo - 0 (13) 

For collision avoidance, it is a must that at all times, for the closest point on 
the arm. 


QliO 


(14) 


3.2 Distance Functions 

Distance function Dj is defined as the function Q|(x,y)from the 
closest point (x, y)on the arm to the i-th obstacle. 

The position on each link can be identified by 


x=xi+*(x2-xt); y=yi+X(y 2 -yi) 


05) 


in terms, x, y are functions of a parameter X. xj, y lt x 2 . y 2 are Cartesian 
coordintes at end points of the links. 

Substituting (x, y). Q s (x, y) becomes a function of parameter X. To 
find the closest point from the arm to the obstacle, we take differentiation 
and find minimum Qj versus X 


d& 

dX 


06) 


then, t) | is choosen among the closest points on links to the obstacle. In 
most of the avoidable obstacle cases, The “Sosert 'point happens to be on the 
forearm at end-effector. 

When Dj =0, it means the arm touches the i-th obstacle at the closest 
point. When Oj is infinitesimally small, it means the arm grazes the i-th 
obstacle. 


Virtual Potential Field 
The penalty function Pj 

Penalty Function 
is defined as 

Method (Pi) 

Sj= expfDi/aO-l 


(17) 


S| 2 l 

(18) 

Pi« r 

S| < E 

(19) 


*i l« a small number which denotes the dramatic steepness factor between 
the trajectory and the i-th obstacle where the penalty becomes active. T is 
a huge number on the edge of the precision boundary that causes 
computer overflow, e is a u'ny number on the edge of the precision 
boundary that causes computer underflowi The merit of this infinite 
penally function Is by choosing a small number aj, one can define how 
close the trajectory is allowed to dear the i-th obstacle. By increasing the 
value a], one can supplant the steepness of the penalty function so the 
trajectory will never get into the obitades T forbidden are*. As D increases, 
P sharply decreases, i.e, almost no penalty in farther distance; as D 
decreases, P dramatically increases, i.e. a sudden increase of a penalty 
barrier in the goal function for obstacle avoidance. As soon as P dominates 
the goal function, the problem changes from a minimum-goal one Into an 
obstacle avoidance one. See Fig. 4. as following: 



Fig. 4. Penalty function versus distance function diagram 

For this optimal control system, we have Formulated 0,to,a as state 
variables x; u as control variables. For the time-optimal problem, the 
performance index is: 


I = n + f 1 « P«(t) dT (20) 

JO 

P.W=lP|<t> (2>) 

i-t 

Boundary conditions art initial states x(0), and final stales x(l) of specified 
values. Once the slates and the controls are computed, the required 
reacting torques T can be solved from Eq. (1). 

3.4 Violation Compensation Penalty Function Method (P2) 

The penalty function P| is defined as 


P|= 0 

D| i 0 

(22) 

Pi = (D, - c) s 

D| < 0 

(23) 


e is a small number. This penalty is a negative compensation function via 
the distance function. The merit of this penalty function Is to force the 
violation of the obstacle constraint out as the negative sign indicates. 

For this optimal control system, we have the same state variables 
*nd control variables as above. For the time-optimal problem, the new 
performance index is: 


I =« + f 1 n P*(t) dt 
Jo 

(24) 

p.eo= lw,Pi(t> 

(25) 


i-1 


W | is a weighting factor for the corresponding penalty function. 

3.5 Variables Transformation Method (P3) 

For collision avoidance, by introducing a new variable t, 

Ql = *o(*-*o) 2 + bo(*‘*o)(y-yo) + co(y-yo) 2 + fo “ s 2 (26) 

1 ■ 1 ao(x-xo)I + bo(x-xo)$/2+bo(y-yo)t/2 + co(y-yo)J ]/z 

(27) 

where I, ire the lime diffrentiation of x, y. We add one or more 
differential constraints to the control system. For this optimal control 
system, we have formulated 0,co,a,zit state variables x; u at control 
variables. For the time-optimal problem, the performance index is: I m n 


Ztro 
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3.6 Time Scaling r f ,_ ft 

In the above system, , lime ha. been normal, zed from § 

lfin.l=l via lhc foIlowin 8 t=*T, i.e. . a 

parameier which represents the final lime. 


3.7 Minimum Distance Problem 

For the minimum -distance problem of the end-effector in Cartesian 
space, the performance index can be replaced by: 

I = f l n (v*2 + \ y 1)\/2 dx + f 1 « P*(t) dx ( 28 ) 

Jo * o 


3.8 Primal Formulation 

Optimal control has the characteristic of duality 
study, the sequential gradient restoration algorithm 
conjunction with primal formulation. 


[41, 42]. In this 
is employed in 


3 . _ m , . ■ 

The sequential gradient restoration algorithm, in either the primal 
formulation or the dual formulation, i. an iterative technique which l. 
conitructcd by a sequence of two-pha.e suboptimal cycles. Each cycle 
includes a gradient phase and a reiteration phaae. In the gradient phase, 
the value of the augumenled functional is decreased in one step, whi e 
avoiding excessive constraint violation. In the restoration phase, the value 
of the constraint error is decreased in one or multiple steps, while .votdin, 
excessive change in the vxlue of the functional. In a complete gradient- 
restoration cycle, the value of the functional is decreased, wh.le the 
constraints are satisfied to a pre-selected degree of accuracy. Therefore. . 
sequence of suboptimal lolulioni is generated Each new suboptimal 
solution is an improvement of the previous one from the point of view for 
the value of the functional to he minimized. The optimal solution ll 
reached when the optimality error and the constraint error are both 
satisfied to a certain accuracy. Schematic diagram is shown in Fig. 5: 


4. Optimal Control Theory 

The optimal control problem [43] is described in general as follows: 
With respect to the vectorial state variable x(t), vectorial control 
variable u(t) and the vectorial parameter n, the problem of minimizing a 
functional 

I = f 1 f(x,u,n,t)dt +[h(x,«)]o+[&(x.*)]l ( 29 ^ 

Jo 

subject to differential constraints: 

*-4(x,u,n,t)=0, Oils I, (30) 

initial conditions: 

[to(x,n)]o=0, (**1) 

and final conditions: 

[y(x,n)l,=0. OD 

where f, h, g. are scalar functions, and 4, w, y vectorial functions of 
specified dimensions, t is a independent variable. The subscript 0 denotes 
the initial point, and the subscript 1 denotes the final point. 

Optimality Criteria 

By introducing the Lagrange multipliers, the problem shown above 
can be recast as minimizing the augmented functional J 

J = I + L (33) 

subject to Eqs. (30-32), where L it the Lagrangian functional 

L = J 1 xT(x-4(x,u,7i,t))di + (o T co)o+ (P T V)1 ( 34 ) 

The symbols X(t), a, p denote Lagrange multipliers of appropriate 
dimensions associated with the constraints. The superscript symbol T 
denotes the transpose of the matrix. 

The first-order oplimalily criteria originated from Pontryagm 
Extremum Principle for Eqs. (29-34) can be derived from Euler equat.ons 
in calculus of variation as: 
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Let x(t), u(t), it denote the nominal functions; let x(t), u(t), n , denote 
the varied functions; let Ax(t), Au(t), A« denote the displacements leading 
from the nominal functions to the varied functions. Under the assumption 
that the displacements Ax(l), Au(t), An are linear with stepsize a, where a > 
0; and A(t), B(t), C denote the displacements per unit stepsize. Then the 
following relations can be used for iterations: 


x(t) = x(t) - 1 - Ax(t) = x(t) + oA(t) 


(40) 


X-f, + 9x>-=0. Ottil, 

(35) 

f.-*uJt=0. Oilil. 

(36) 

f 1 (f«-4,x)dt ♦ (h, + <a,o)o + (g* + VxP)l=0, 

Jo 

(37) 

(-X + h x + rox<T)o=0, 

(38) 

(X + g x + ynF)l=°- 

(39) 


u(t) = u(t) + Au(t) = u(t) ♦ aB(t) (41) 

n = it + An = n + aC (42) 

Thus, each iteration of the gradient phase and the restoration phase 
involves two distinct operations: (i) the determination of the direction 
functions A(t), B(t), C, and (ii) the determination of the stepsize of variation 

CL 


In terms, we seek the functions x(l), u(t), * and the multipliers MO, a, 
u such that the feasibility Eqs. (30-32) and the optimality criteria Eqs. (35- 
39) are satisfied to certain numerical accuracy. 
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From (40-42) and constraint conditions (30-32), one can derive the 


following relations from first order variation: 

A - ** t A - ♦uTB -*JC + D r (S-#) = 0, 0i t i I, (43) 

(«x T A + cc* T C + Dro))o =0, (44) 

(Yx T A + y, t C + Dry)i =0, (45) 

and from Eqs. (40-42) and first-order optimality criteria (35-39), one can 
derive the following relations from first order variation: 

J - D g f* + + x \ = 0, Ottsl, (46) 

B + Djfu • ^yX= 0, Oittl, (47) 


C + Jo + (VnP)i + D g [ J Q l F w dt + (h*)o + (g*)i ] = 0, 



(48) 

(A - X + +■ Dgh x )o — 0, 

(49) 

(X + YxM + D g gx)i = 0, 

(50) 

where, in the gradient phase, D g = l, D r = 0, 

(51) 

in the restoration phase, D g = 0, D r = 1. 

(52) 

The above linear two-poim boundary-value problem 
be solved for the direction functions A(t), B(t), C, by 
particular solutions [44, 45], 

[LTP-BVP] can 
the method of 

Stepsize 

Eqs, (40-42) define one-parameter functions of the 
this parameter, the functionals I, J, P become functions of a 

stepsize a For 
as following: 

r = r<a) J = J (a) P=P(a) 

(53) 

Then, bisection technique is used for the one-dimension search to 
find the stepsize, starting from reference stepsize a g in gradient phase, 
until (i) 

T(a)<J(0), P(a) < P, p 

(54) 


where, N denotes quadratic norm operation. 

Thus, numerical convergence for optimal solution can be declared 

when 


P 1 £1, 

(59) 

Q i e 2» 

(60) 


£{ , E2 are preselected, small, positive numbers. 

The algorithm is started from providing nominal functions of u(t), 
and n. The nominal functions can be provided arbitrarily, but good 
nominals help convergence. The nominal controls are provided with a 
standard shooting method of Modified Quasilinearization Algorithm, 
followed by solving the nominal states based on nominal controls, to some 
accuracy of terminal conditions. 

Then, the restoration phase is started. Eqs. (43-45) are solved with 

(52) and search of stepsizc in restoration phase. The one or more iteration 
restoration phase is completed only until Eq. (59) is satisfied. Then, the 
gradient phase is started. Eqs. (46-50) are solved with (51) and search of 
stepsize in gradient phase until Eq. (60) is satisfied for only one iteration. 
The restoration phase is started again. Thus, a sequence of suboptimal 

solutions is generated. Each new solution is an improvement of the 
previous one from the point of view for the value of the functional to be 
minimized. The optimal solution is reached when Ineqs. (59-60) are both 

satisfied. 

6. Numerical Examples 

Numerical examples for tfme-optlmal control wjth obstacles avoidance 

schemes of a two-link robotic manipulator are shown in this section. The numerical 
and analytical aolutlons of time-optimal control without obstacles can be refered to 
[22], The following physical parameters are taken from Asada [461, Sahtr and 
Hollerbach [16] and 2iang and Wang [20]. The obstacle Is put on the midway, or even 
further Inward, of the previous no-obstacle optimal trajectory on purpose. The 
algorithm can be applied to any degree-of-freedom robots with arbitrarily given 
physical parameters and boundary conditions. 


In joint space, 

initial position (01 . 02)i * (0.23, 0.35) rad. 

final position (01.02 >f * (0-8208, 1.4208) rad. 

initial velocity («i,et2>i* (0-0, 0.0} rad/sec, 

final velocity («i,fi>2)f - (0-0, 0.0) rad/sec. 

acceleration bounds (Cj, C 2 ) - (0.5, 1.0) rad/{sec) 2 . 

gravity constant g ■ 9.8 m/(sec) 2 . 


P* U a preselected number, not necessarily small; and starting from 
reference stepsize cif in restoration phase, until (ii) 

P(a) < P(0) 


Link 1 

mass m] ■ 50 kg, length 1) » 0.5 m, moment of inertia Ii * 5.0 kgZ(m) 2 . 
Link 2 

mass m2 * 30 kg, length 12 * 0.5 m, moment of inertia I 2 - 3.0 kg/(ra) 2 . 


In a complete, successful gradient-restoration cycle, the followin, 
condition must be satisfied or the cycle is restarted with reduced stepsize. 


The ellipse obstacle is represented by the following equation: 

Q - ao(x-xo) 2 + bo(*-*o)(y-y0) + co(y-yo) 2 + fo - 0; where, fo * -(ro) 2 


where lj denotes the value of the functional (29) after current cycle. Ij.t 
denotes the value of the functional (29) after the previous cycle. 

Updating suboptimal solution schemes 

Once the direction function A(t), B(t), C, and stepsize a arc solved, the 
ftates, the controls, and the parameters are updated according to Eqs. (40- 


Summary of Algorithm 

Let P be the square norm of the error associated with the feasibility 
Eqs. (30-32), and Q be the square norm of the error associated with the 
optimality criteria Eqs. (35-39), then 

P ~ Jq ^ ( *-4)dt 4 N(co)o + N(y)i (57) 

Q = Jq - fx + $xX)dt + N(f u - 4 u X)dt 

+ N[ jjj (fK-**X)dt + (h* + ( 0 , 0)0 + (g* + Y*f)i I 

+ N(-X + h* + N(X + g* + (58) 


The following symbols are used in the tables: 

PI: Virtual Potential Field Penalty Function Method 
P2: Violation Compensation Penalty Function Method 
P3: Variables Transformation Method 
E : Ellipse Obstacle. C : Circle Obstacle, when bo = 0 


Table 1. Comparison of Obatacles Avoidance Schemes 



(PIC) 

(P2-C) 

(P3-C) 

*0 (m) 

0.5 

0.5 

0.5 

yo (rn) 

0.76 

0.76 

0.76 

r 0 (35) 

0.1 

0.J 

0.1 

■0 

1.0 

1.0 

1.0 

to 

0.0 

0.0 

0.0 

CO 

1.0 

t.O 

1.0 

a (nj ) 

SO' 4 

- 

- 

e (m) 

- 

3.0x10 4 

- 

W 

- 

0.5xl0 10 

- 

minimum 




time (aec) 

2.914 

3.071 

4.337 


*** denow* there is no such value for the .theme 

2.137 sec is die mini mum time without obstacle avoidance. 
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aM aw .wtakh. irmiforMUM him N» .ImU rtwd. Aw-O) ard * ab * l * ct * h 

. « («.J. 0.74) «( ndlu 0.1. Vlmtl pw—tfl ««W P~U> h»«l» to» «l» 
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TiMt X CempariMO of DUtete 

Locate o t Ellipse and Circle Obctaeh 

PI 

(HI) 

(W) 

(Cl) 

(Cl) 

(■) 

O.Sl 

0.50 

0.51 

0.50 

*•<■> 

0.42 

0.74 

0.42 

0.74 

ro (■) 

0.2 

0.2 

0.1 

0.1 

SO 

2.0 

2.0 

1.0 

1.0 

bo 

2.0 

2.0 

0.0 

0.0 

•0 

2.0 

2.0 

1.0 

t.o 

• (■> 

10* 

to’ 4 

iV*"” 

to* 4 

minimum 
tec (aoc) 

4.332 

3.330 

3.931 

1.914 

XI 97 sm 1* *e mlai—m 

ta witetf oWtoeta wiH-w. 


la T.M. J„ ik. clieh .nd .UIpM »b«ael« •»**•»“ *" ltawl ,n »ld.-ky-»ld. 
aaiata. A, w. H>. EUlpM (ro-0.7) I a l“l« I" lla Ik. Ckd. (ro-#»- L 00 * “** 
i, 43 kpa ctock.iK griaud. Ink El. Cl h.v. Ik. earn toeula .1 (0.51. 

0 . 01 ) ^ a «». da. lo d» ™*« U- Bl, Cl rtKh lav, »«. oaa l««ia u 

(0.5. 0.70). Owlao » Ik. att. Jan-y *» ■»•«->“ «*•*• *• «•*“ 

a«al. « Ik. tkar Ik. *«al., Ik. toflt« UK *inia.*-li». to. to kidney 


Ike Ik. -*■ d totof, Ik. raalu e a wapad. to ik. finl .llipa, (El) to TtbU X 
ihm In FI i*. Wl. 

Fig. 6 oontilu te optimal trajectory in *Ute»» *i** 

T\g. 7 Canute the Join angle profile In minimum te*. 

Pig. I the joint velocity profile ta minimum tlue. 

Pig. 9 coculne the joint acceleration profile in minimum time. 

Fig. 10 contain* the toffee prcfll* In minimum tlue. 

Pig. II contains the teunoe Autecc proflle tat minimum tec. 

Table 3. Comparted of DUVm late* ef Circle Obeucte. 


PI 

(Cl) 

(Cl) 

(C3) 

*0 (■) 

1.0 

1.0 

1.0 

90 (■) 

1.0 

i.O 

1.0 

ro (■) 

0.4 

0.5 

0.4 

*0 

1.0 

1.0 

1.0 

bo 

0.0 

0.0 

0.0 

«0 

1.0 

1.0 

1.0 

■ <■) 

to* 4 

0.5a 10* 4 

to- 4 

minimum 
tec (ace) 

3.129 

2.696 

XI 37 


(CJ) oteuie i» MteUe the pwiow uo-ahet seU vfimtl wjwwy. 
1.197 m« to te arfrimum tte withoel obstacle avsUaaea. 


hi Table 3., the circle U moved te center location (1. 1). Hedies ef the circle Is varied 
M (Cl, C2, CSHD.t, 0.5, 0.4). In C3 case, the ebetacle is oetside the work envelop of te 
robot, so the n Irvin aa-Unt for Ci U the sue as te one without obstacle The 
minim urn-tee Is longer as te obstacle Is bigger in Cl, Cl eases. 

In C3 case, owing to te influence of te satiating penalty function, there are two 
carves overlapped on te trajectory One curve U for previous optimal trajectory 
without obstacle avoidance scheme, another ooe U eptinal trajectory with obstacle* 
avoidance scheme and obstacle is outside te work envelop. 


4.1 lever* Obstacle Avoidance 
In Joint space, 

initial position Pi, *2)1 - (0.349, 0.421) rad. 

final portkm (*i.»2)f - (0.497. 0.155) rad. 

Initial velocity (*], »2)i - (00. 0.0) rad/ sec. 

final velocity («l.m2)f - <00, 0.0) rad/see. 


The following case study shows: the arm started near the edge of one side of the 
obstacle and ended near te edge of another side of the obstacle. 

Table 4. Severe Obstacle Avoidance. 



(Pl-cj 

(P2-C) 

SO (01) 

0.667 

0.667 

90 (m> 

0.667 

0.667 

ro (®) 

0.1 

0.1 

•0 

1.0 

10 

bo 

0.0 

0.0 

co 

1.0 

1.0 

a (m) 

10‘ 3 

- 

« (») 

- 

0.2x1a' 1 

W 

- 

0.5x10* 

minimum 
time (sec) 

2.800 

2.911 

Jaixnu than 

t.oaa hc is th« 

Is no such vtlue for (he i theme, 
minimum time without obstacle avoidance. 


As we see, minimum-time control is not necemrily related to minimum-distance of 
the end-effector. The collision avoidance scheme has excellence to move around and 
■void severe obstacle. 

7 . Discussion 

The Insights of the merit of the optimal obstacles avoidance are 

shown above in Fig. 6-15. All the obstacle avoidance trajectories have the 
following characteristics: 1) grazing by the obstacle. 2) trying to achieve 
previous no-obstacle trajectory at near bang -bang control for minimum- 
time. (At least one joint bang-bang control is the solution for minimum- 
time without obstacles avoidance) 3) achieving previous no-obstacle 
optimal trajectory with collision avoidance scheme in the cases of no- 

obsttclei. 4) being able to move around and avoid the severe obstacle. 

Virtual potential penalty function method does not cause obstacles 
constraints violation or over-constrained situation, is the one and only best 
method. Violation compensation method is difficult to implement owing to 
the two weighting factors which causes a little obstacle constraint violation 
from time to time. Variables transformation method is over-constrainted 
when the obstacles are away from potential collision. This discussion 
matches the one in [4] even though the implementation of penalty function 
is different. This approach also illustrates the experimental results for 

optimization with inequality and/or equality constraints. 

As we can see from the comparison tables, the minimum-time of 

collision-free optimal trajectory is relatively depended on the size and 

location of the obstacles. The jerk control can be overcome by achieving a 
near optimal motion in which the trajectory is farther away from the 

obstacle and the minimum-time is longer. 

More intensive research need to be done on minimum distance 
finding through optical devises or solid modeling. Since the robot 
manipulators are usually constructed by connected links, in most of the 
cases, we can say obstacles avoidance for fixed configuration is equivalent 
to the end-effector obstacle avoidance even though this statement is not 

true in general. We have to solve end-effector obstacles avoidance before 
we solve other type of problems because the object is usually on the grip. 

The numerical experiments have been done on IBM AS9000 
mainfraim and VAX 8800, are also attempted to be done on Macintosh. The 
CPU time for current research varies from 20 minutes (IBM) to one hour 
(VAX). The accuracy also varies from machine to machine without very 
much difference. As the computer industries are growing, the CPU time or 
accuracy is not a problem for future scientific computation 

The numerical results are constructed by 100 cycles and 300 
iterations whichever reached first. The convergence is fast at early stages, 
it slows down after the sub-optimal solutions come dose to the optimal 
solution. To save computation, one can set up lower limits for cycle, 
iteration and CPU time, so near-optimal solutions will be generated based 
on Pontryagin Extremum Principle. 
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8. Conclusion 

In this paper, collision free optimal motion and trajectory planning 
for robotic manipulators arc solved by a method of sequential gradient 
restoration algorithm. Numerical examples of a two degree-of-freedom 
robotic manipulator are demonstrated. The obstacle is put on the midway, 
or even further inward, of the previous no-obstacle optimal trajectory on 
purpose. For trying to achieve previous no-obstacle trajectory, the 
trajectory tangentially grazes by the obstacle and the minimum-time 
motion successfully avoids the obstacle. The minimum-time is longer for 
the obstacle avoidance cases than the one without obstacle. All the 
numerical experiments indicate the obstacles avoidance scheme has the 
same characteristics which allows the trajectory gets as close to the 
optimal as possible but barely graze by the obstacle. The weighting and 
effective point of the penalty can be defined by one parameter which 
justify the closeness between the trajectory and the obstacle. The 
trajectory will try to achieve optimization under the obstacles barrier. This 
is the most outstanding characteristic than other schemes to achieve 
collision avoidance and also find the optimal motion without extra 
excursion. 

The obstacles avoidance schemes can deal with multiple obstacles in 
ellipsoid forms by continuously controlling the closest point from the arm 
to the obstacle using virtual potential fields as penalty function! via 
distance functions. The algorithm is very promising in solving collision -free 
optimal control problems for any degree-of-freedom robotic manipulators 
with any performance indices and mobile robots as well. The minimum - 
time motion is at least one joint bang -bang control or near bang-bang 
control with obstacles avoidance, no matter the controls are imposed on 
angular accelerations or on actuator torques. The minimum-distance 
trajectory without obstacles is a straight line. 

Since this algorithm generates true local minimum solution based on 
Pontryagin extremum principle, rather than based on approximations, the 
results provide a benchmark against which any other optimization can be 
measured. 

The perspective research is to investigate the result of optimal 
solutions for robotic manipulators when the controls are imposed on 
actuator constraints, and/or with moving obstacles avoidance under 
different performance indices; and model-reference adaptive optimal 
feedback control. 
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G *re functions which depend on the configuration (like: elbow-down). For 
a two-link robotic manipulator in elbow-down position (Fig.l.): 

wh '"' (70) 

(71) 

(ai } \ ( l2COs(ei+9i) 12*111(81^62) ^ 

[«92 J ~ lil2sin02\ > -Ucos0i-l2COi(8i+02) -li»in 9 i -l2*in(0i + 02) J 

*£) (72) 

{ “ l V 1 p 2 co< ( e l + Q 2) l2*in(©i + 02) V** \ 

(aj+aj j Iil2sin02v -UcosOi -li*in0j /.*y ) 

1 r ilhco.92 l2 2 Y “ ,J , 1 m\ 

* lil2»ine 2 ^ -tl 3 -I1I2COSS2 Y ( “ 1+0>2 ^ > 
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9. 

Forward kinematics x(t)=F](9(t)) 


(61) 


v(t)=F2(0(O. <*>(t)) 


(62) 

10. 

■ (t)=F3(0(t), to(t), a(t)) 


(63) 

11. 

where x(t), v(t), and a(t) are vectors of positions, velocities and 
accelerations of the end-effector in Cartesian space. 0(t), <o(t), and a(t) are 
vectors of angles, angular velocities and angular accelerations in joint 

12. 

space. 

F are functions. For a two-link planar robotic manipulator [40] (Fig.l.): 
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